#!/usr/bin/env python

# --------
# Imports:
# --------
# Project configuration
from config import *

# Ros imports:
import roslib; roslib.load_manifest(NODE_NAME)
import rospy
from geometry_msgs.msg import PoseStamped 
from threading import RLock

# Other python imports
from math import floor, ceil

# -----------
class SlamMap:
# -----------
	# ------------------------------------------------------------------------------------------------
	def __init__(self, widthPx, heightPx, metersPerPixel, mapCenterPx, mapCenterWorld, mapHeadingWorld):
	# -------------------------------------------------------------------------------------------------
		""" Map constructor """

		# Map constants
		# -------------
		self.widthPx = widthPx 					# The width of the map (in pixels)
		self.heightPx = heightPx				# The height of the map (in pixels)
		self.metersPerPixel = metersPerPixel	# The number of meters per pixel
		self.mapCenterPx = mapCenterPx			# The map's center point (in pixels)
		self.mapCenterWorld = mapCenterWorld	# The map's center point (real world)
		self.mapHeadingWorld = mapHeadingWorld 	# The map's orientation (real world)

		# Key datastructures
		# ---------------------
		# Initialize the map to all unknown
		self.slamMap = [[{} for x in range(self.widthPx)] for y in range(self.heightPx)]

		# Initialize the locks for the map cells
		self.mapLocks = [[RLock() for x in range(self.widthPx)] for y in range(self.heightPx)]

	# --------------------
	def getLock(self, key):
	# --------------------
		""" Gets the lock for the given key """
		return self.mapLocks[key[1]][key[0]]

	# ------------------------
	def __getitem__(self, key):
	# ------------------------
		""" This overloads the [] operator. If the key is an integer, then it returns the row of the map
		(so that you can then access the element by using the [] operator on the row). If key is a tuple 
		then return the actual element itself. """

		with self.getLock(key):

			# Make sure the key is in bounds
			if self.mapInBounds(key):
				# Return the map element
				return self.slamMap[key[1]][key[0]]
			else: raise IndexError('Key out of bounds')

	# ---------------------------
	def worldToMap(self, worldPt):
	# ---------------------------
		""" Convert world coordinates (represented as a 2-tuple) to map coordinates """
		mapCenterPx = self.mapCenterPx
		mapCenterWorld = self.mapCenterWorld
		metersPerPixel = self.metersPerPixel
		return (int(ceil(mapCenterPx[0] + ((worldPt[0] - mapCenterWorld[0]) / metersPerPixel))),\
				int(ceil(mapCenterPx[1] - ((worldPt[1] - mapCenterWorld[1]) / metersPerPixel))))

	# -------------------------
	def mapToWorld(self, mapPt):
	# -------------------------
		""" Convert map coordinates (represented as a 2-tuple) to world coordinates """
		mapCenterPx = self.mapCenterPx
		mapCenterWorld = self.mapCenterWorld
		metersPerPixel = self.metersPerPixel
		return (((mapPt[0] - mapCenterPx[0]) * metersPerPixel) + mapCenterWorld[0],\
			    ((mapCenterPx[1] - mapPt[1]) * metersPerPixel) + mapCenterWorld[1])

	# -----------------------
	def mapInBounds(self, pt):
	# -----------------------
		""" Determines whether or not a map point is in bounds """
		return pt[0] >= 0 and pt[0] < self.widthPx and pt[1] >= 0 and pt[1] < self.heightPx

	# ---------------
	def __str__(self):
	# ---------------
	# Map toString method
		return self.slamMap.__str__()
